#ifndef BASE_CONTROLLER_STM32_H_
#define BASE_CONTROLLER_STM32_H_


#include <ros/ros.h>
#include <ros/console.h>
#include <serial/serial.h>
#include <tf/tf.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>

using namespace std;

#define SEND_DATA_CHECK     1          //发送数据校验标志位
#define READ_DATA_CHECK     0          //接收数据校验标志位
#define RECEIVE_DATA_SIZE   24         //下位机发送过来的数据的长度
#define SEND_DATA_SIZE      11         //ROS向下位机发送的数据的长度
#define FRAME_HEADER        0X7B       //帧头
#define FRAME_TAIL          0X7D       //帧尾
//与IMU陀螺仪设置的量程有关，量程±500°，对应数据范围±32768
//陀螺仪原始数据转换位弧度(rad)单位，1/65.5/57.30=0.00026644
#define GYROSCOPE_RATIO     0.00026644f
//与IMU加速度计设置的量程有关，量程±2g，对应数据范围±32768
//加速度计原始数据转换位m/s^2单位，32768/2g=32768/19.6=1671.84
#define ACCEl_RATIO 	    1671.84f 
#define SAMPLING_FREQ       20.0f // 采样频率

//速度、位置数据结构体
typedef struct __Vel_Pos_Data_
{
	float X;
	float Y;
	float Z;
}Vel_Pos_Data;

//下位机向ROS发送数据的结构体
typedef struct _RECEIVE_DATA_     
{
	    uint8_t rx[RECEIVE_DATA_SIZE];
	    uint8_t Flag_Stop;
		unsigned char Frame_Header;
		float X_speed;  
		float Y_speed;  
		float Z_speed;  
		float Power_Voltage;	
		unsigned char Frame_Tail;
}RECEIVE_DATA;

typedef struct _SEND_DATA_  
{
	    uint8_t tx[SEND_DATA_SIZE];
		float X_speed;	       
		float Y_speed;           
		float Z_speed;         
		unsigned char Frame_Tail; 
}SEND_DATA;

//IMU数据结构体
typedef struct __MPU6050_DATA_
{
	short accele_x_data; 
	short accele_y_data; 	
	short accele_z_data; 
    short gyros_x_data; 
	short gyros_y_data; 	
	short gyros_z_data; 

}IMU_DATA;

//协方差矩阵，用于里程计话题数据，用于robt_pose_ekf功能包
const double odom_pose_covariance[36]   = {1e-3,    0,    0,   0,   0,    0, 
										      0, 1e-3,    0,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0,  1e3 };

const double odom_pose_covariance2[36]  = {1e-9,    0,    0,   0,   0,    0, 
										      0, 1e-3, 1e-9,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0, 1e-9 };

const double odom_twist_covariance[36]  = {1e-3,    0,    0,   0,   0,    0, 
										      0, 1e-3,    0,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0,  1e3 };
										      
const double odom_twist_covariance2[36] = {1e-9,    0,    0,   0,   0,    0, 
										      0, 1e-3, 1e-9,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0, 1e-9} ;

namespace base_controller{

class SerialParser{
public:
    SerialParser(string port, int baud);
    ~SerialParser();

    void SerialClose();
    unsigned char Check_Sum(unsigned char Count_Number,unsigned char mode);     //BBC校验函数
    short IMU_Trans(uint8_t Data_High,uint8_t Data_Low);    //IMU数据转化读取
    float Odom_Trans(uint8_t Data_High,uint8_t Data_Low);   //里程计数据转化读取
    void SetMotorSpeed(double x, double y, double z);
    bool GetSensorData();

    RECEIVE_DATA Receive_Data; 
    SEND_DATA Send_Data;  
    IMU_DATA Imu_Data;  
    Vel_Pos_Data Robot_Vel;    //机器人的速度 
    float Power_voltage;       //电源电压


private:
    string serial_port_;
    int serial_baud_,serial_timeout_;
    serial::Serial serial_;
    bool serial_haserr_;

	vector<uint8_t> rec_data;
	bool start_record = false;
};

class BaseController{
public:
    BaseController();
    ~BaseController();

    void OdomReset();
    void ControlCore();
    void Publish_Odom();        //发布里程计话题
    void Publish_Voltage();     //发布电源电压话题
    void Publish_Imu();         //发布IMU话题

private:
    ros::NodeHandle nh_;
    ros::Publisher odom_pub_,imu_pub_,voltage_pub_;
    ros::Subscriber cmdvel_sub_;

    void cmdvel_cb(const geometry_msgs::Twist::ConstPtr& vel_msg);

    string serial_port_,base_frame_,odom_frame_,imu_frame_;
    int serial_baud_;
    double robot_x_,robot_y_,robot_yaw_;
    
    ros::Time time_now_,time_last_,speed_time_last_;
    float Sampling_Time; 

    SerialParser* serial_parser_;  
	bool odom_tf_;
};



}


#endif